#ifndef SENSOR_KALMAN_H
#define SENSOR_KALMAN_H

#include "sensor_include.h"


// TYPEDEF

typedef struct
{
    uint8 u8_count;
    fixed32 f32_r;
    fixed32 f32_p[2][2];
    fixed32 f32_x[2];
    fixed32 f32_q;
    fixed32 f32_k[2][2];
} kalman_filter;

// Public function declaration

void KF_Initialize( kalman_filter *tp_filter );
void KF_Reset( kalman_filter *tp_filter );

void KF_AdjustR( kalman_filter *tp_filter,
                 fixed32 f32_noise );
void KF_AdjustQ( kalman_filter *tp_filter,
                 fixed32 f32_diff1,
                 fixed32 f32_diff2 );
fixed32 KF_Update ( kalman_filter *tp_filter,
                    fixed32 f32_input0,
                    fixed32 f32_input1 );


#endif // SENSOR_KALMAN_H
